Optimize Waypoints